/*
 * Robot.cpp
 *
 *  Created on: 23.11.2011
 *      Author: jan
 */

#include "Robot.h"

namespace std {

Robot::Robot(Maze* maze) : speed(0.02),m(maze){
	this->heading = 90;
	chose_random_direction();
	this->p = maze->random_free_place();
	this->x = p.x;
	this->y = p.y;
}

void Robot::chose_random_direction(){
	this->heading = (UNIFORM_RAND)*360;
}

void Robot::move(){
	int step_count = 0;
	while( true ){
		if(!advance_by(speed,m,true)){
			Point p (x+dx,y+dy);
			chose_random_direction();
//			cout << "new direction: "<<heading<< " pos: ["<< p.x << ";" << p.y << "]"<< " free?"<<m->is_free(p)<< endl;
			step_count++;
		}else{
//			Point p (x+dx,y+dy);
//			cout << "new direction: "<<heading<< " pos: ["<< p.x << ";" << p.y << "] xy:["<<x<<";"<<y<<"]"<<" dxdy ["<<dx<<";"<<dy <<"] free?"<<m->is_free(p)<< endl;
			return;
		}
	}
}

double Robot::getHeading(){
	return heading;
}

double Robot::read_sensor(){
	return add_little_noise(Particle::read_sensor(m));
}

Robot::~Robot() {

}

} /* namespace std */
